05. Waypoint Updater Node (Partial)

Waypoint Updater Node Overview

Before the car can move in the simulator, it will be necessary to write a first version of the waypoint_updater node. You can find class and method stubs for the node in 

(path_to_project_repo)/ros/src/waypoint_updater/waypoint_updater.py

The eventual purpose of this node is to publish a fixed number of waypoints ahead of the vehicle with the correct target velocities, depending on traffic lights and obstacles. The goal for the first version of the node should be simply to subscribe to the topics

  • /base_waypoints
  • /current_pose

and publish a list of waypoints to

  • /final_waypoints

The /base_waypoints topic publishes a list of all waypoints for the track, so this list includes waypoints both before and after the vehicle (note that the publisher for /base_waypoints publishes only once). For this step in the project, the list published to /final_waypoints should include just a fixed number of waypoints currently ahead of the vehicle:

  • The first waypoint in the list published to /final_waypoints should be the first waypoint that is currently ahead of the car.
  • The total number of waypoints ahead of the vehicle that should be included in the /final_waypoints list is provided by the LOOKAHEAD_WPS variable in waypoint_updater.py .

The next section includes details about the message type used to publish to /final_waypoints .

Waypoint Message Descriptions

This section will demonstrate use of rostopic and rosmsg to learn more about the messages being transmitted on /base_waypoints and /final_waypoints . If you are already familiar with these ROS commands and feel comfortable exploring ROS messages on your own, feel free to skip this section!

From the code in waypoint_updater.py , we can see that both the /final_waypoints and /base_waypoints topics have message type Lane . You can look at the details about this message type in <path_to_project_repo>/ros/src/styx_msgs/msg/ , but this can also be done from the command line after launching the ROS project using rostopic and rosmsg as follows:

After opening a new terminal window and sourcing devel/setup.bash , you can see see all topics by executing: 

$ rostopic list

You should see /final_waypoints among the topics. Executing: 

$ rostopic info /final_waypoints

will provide info about the message type being used in /final_waypoints : 

Type: styx_msgs/Lane

Next executing: 

$ rosmsg info styx_msgs/Lane

provides the following message information: 

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
styx_msgs/Waypoint[] waypoints
  geometry_msgs/PoseStamped pose
    std_msgs/Header header
      uint32 seq
      time stamp
      string frame_id
    geometry_msgs/Pose pose
      geometry_msgs/Point position
        float64 x
        float64 y
        float64 z
      geometry_msgs/Quaternion orientation
        float64 x
        float64 y
        float64 z
        float64 w
  geometry_msgs/TwistStamped twist
    std_msgs/Header header
      uint32 seq
      time stamp
      string frame_id
    geometry_msgs/Twist twist
      geometry_msgs/Vector3 linear
        float64 x
        float64 y
        float64 z
      geometry_msgs/Vector3 angular
        float64 x
        float64 y
        float64 z

From here you can see that the messages contain a header and a Waypoint list named waypoints . Each waypoint has pose and twist data. Going further, you can see that twist.twist data contains 3D linear and angular velocities. For more information about twist messages, see documentation here .

Check to be sure you can explore the messages sent in /base_waypoints and /current_pose as well using the steps above!

Lane message example

As a use-case example, given a single styx_msgs/Lane message my_lane_msg , you can access the x direction linear velocity of the first waypoint in Python with: 

my_lane_msg[0].twist.twist.linear.x

Note that the coordinates for linear velocity are vehicle-centered, so only the x-direction linear velocity should be nonzero.

Topics and message types

For convenience, we have provided the following table with topic and message info for this step of the project:

Topic Msg Type Notes
/base_waypoints styx_msgs/Lane Waypoints as provided by a static .csv file.
/current_pose geometry_msgs/PoseStamped Current position of the vehicle, provided by the simulator or localization.
/final_waypoints styx_msgs/Lane This is a subset of /base_waypoints. The first waypoint is the one in /base_waypoints which is closest to the car.

Next Concept